// Includes
// --------
#include <iostream>
#include <vector>
#include <sys/time.h>
#include <cstdio>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/CvBridge.h>

// OpenCV
#include <opencv/cv.h>
#include <opencv/highgui.h>

// Definitions
#define SCALE_FACTOR 1.2

// Namespaces
using namespace std;

// Constants and configuration
const char * NODE_NAME = "filtered_face_detector";

// Helpers
#define START(method) printf("<" #method">\n"); fflush(stdout); timeval TIMER_start; gettimeofday(&TIMER_start,NULL)
#define STOP(method)  do {\
					  timeval TIMER_res, TIMER_stop; gettimeofday(&TIMER_stop,NULL); \
					  timersub(&TIMER_stop, &TIMER_start, &TIMER_res); \
					  double TIMER_duration = (TIMER_res.tv_sec + TIMER_res.tv_usec/1000000.0); \
					  printf("<duration>%f</duration>\n</" #method ">\n", TIMER_duration); \
					  fflush(stdout);} while(0)

// -----------------------
class FaceDetectorProfiler
// -----------------------
{
	protected:
		// Members
		ros::NodeHandle node;
		ros::Subscriber imageSubscriber;
		sensor_msgs::CvBridge bridge;
		sensor_msgs::Image::ConstPtr image;
		cv::CascadeClassifier classifier;

		// Callback
		void imageCallback(const sensor_msgs::Image::ConstPtr& msg)
		{
			this->image = msg;
		}

	public:
		// -------------------
		FaceDetectorProfiler()
		// -------------------
		{
			// Subscribe to the image topic
			this->imageSubscriber = this->node.subscribe("image", 10, &FaceDetectorProfiler::imageCallback, this);

			// Load the cascade
			this->classifier.load("data/haarcascade_frontalface_alt.xml");
		}

		
		// --------------------
		~FaceDetectorProfiler()
		// --------------------
		{
		}

		// -------	
		void run()
		// -------	
		{
			START(frames);

			// Main control loop
			while(ros::ok())
			{
				// Make sure we have an image
				if(this->image != NULL)
				{
					// ---------
					START(frame);
					// ---------

					sensor_msgs::Image::ConstPtr image = this->image;
					
					// Convert the image to an OpenCV Image
					try{
						cv::Mat cvImage(this->bridge.imgMsgToCv(image, "bgr8"));
						
						// Run face detection
						std::vector<cv::Rect> faces;
						this->classifier.detectMultiScale(cvImage, faces, SCALE_FACTOR);
						
						// Print the number of faces
						//cout << "Faces: " << faces.size() << endl;

						// TEMP
						//STOP(frame);
						//break;
						
					}catch(sensor_msgs::CvBridgeException error)
					{
						ROS_ERROR("Error in converting the ros image to a cv image");
					}

					// --------
					STOP(frame);
					// --------
				}
				
				// Spin
				ros::spinOnce();	
			}
			STOP(frames);
		}
};

// ----------------------------
int main(int argc, char** argv)
// ----------------------------
{
	// --------
	START(root);
	// --------

	// Intialize our ROS node
	ros::init(argc, argv, NODE_NAME);

	// Create our profiler node object
	FaceDetectorProfiler profiler;
	profiler.run();

	// -------
	STOP(root);
	// -------

	return 0;
}
